#pragma once

#include "math.h"
#include "vectormath.h"
#include <vector>

namespace Tmpl {

template <typename T>
class Spline 
{
public:
	enum SplineType
	{
		SPLINE_LINEAR,
		SPLINE_CATMULLROM,
		SPLINE_BEZIER_SEGMENTS_4,
		SPLINE_BEZIER_QUAD
	};

	Spline(SplineType type = SPLINE_LINEAR, short approximations = 50) : mType(type), mTotalLength(0.0f), mApproximations(approximations){}
	~Spline(){}

	void Clear(){mPoints.clear();}
	void AddPoint(T point, bool recalc=true){ mPoints.push_back(point); if (recalc) EvaluateLength(); }
	void ErasePoint(int idx, bool recalc=true){ mPoints.erase(mPoints.begin() + idx); if (recalc) EvaluateLength(); }
	void EvaluateLength()
	{
		mTotalLength = 0.0f;
		int points = mPoints.size();
		if (points < 2) return;
	
		// set the segments
		switch (mType)
		{
		case SPLINE_BEZIER_SEGMENTS_4:
			mSegmentLength.resize((points+4)/4);
			break;
		default:		
			mSegmentLength.resize(points-1);
		}

		// calculate segments
		for (unsigned int i=0; i<mSegmentLength.size(); ++i)
		{
			mSegmentLength[i] = GetSegmentLength(i);
			mTotalLength += mSegmentLength[i];
		}
	}

	T GetPointAtTime(float time) const { return GetPointAtDistance(time * mTotalLength); }
	T GetPointAtDistance(float distance) const 
	{
		if (mPoints.size() < 2) return T();
		if (distance >= mTotalLength) return mPoints.back();
		if (distance <= 0.0f) return mPoints.front();
		return GetSample(distance);
	}

	T GetDirectionAtTime(float time) const { return GetDirectionAtDistance(time * mTotalLength); }
	T GetDirectionAtDistance(float distance) const
	{
		T v1 = GetPointAtDistance(Clamp(0.0f, mTotalLength-0.1f, distance-0.1f));
		T v2 = GetPointAtDistance(Clamp(0.1f, mTotalLength     , distance+0.1f));
		v1 = v2 - v1;
		if (v1.SqrLength() > 0.0001f) return v1.Normalized();

		// fail no distance passed
		if (mPoints.size() >= 2) return (mPoints[0] - mPoints[mPoints.size()-1]).Normalized();

		// no possible solution
		return T();
	}

	// Used in spline graphs, get the Y at a position X
	// It assumes that X starts at 0 and goes to 1
	T GetPointAtX(float x) const 
	{ 
		if (mPoints.size() < 2) return T();

		// find nearest x
		int points = mPoints.size();
		float dist = 0.0f;
		for (int i=1; i<points; ++i)
		{
			if (mPoints[i].x >= x)
			{
				float lastPoint = mPoints[i-1].x;
				float pointDist = fabs(mPoints[i].x - lastPoint);
				dist += Lerp(0.0f, mSegmentLength[i-1], fabs(x - lastPoint) / pointDist); 
				break;
			}
			else
			{
				dist += mSegmentLength[i-1];
			}
		}

		// Return the sample point
		if (dist >= mTotalLength) return mPoints.back();
		if (dist <= 0.0f) return mPoints.front();
		return GetSample(dist);
	}
	T GetDirectionAtX(float x) const
	{
		T v1 = GetPointAtX(Clamp(0.00f, 0.95f, x));
		T v2 = GetPointAtX(Clamp(0.05f, 1.00f, x+0.05f));
		v1 = v2 - v1;
		if (v1.SqrLength() > 0.0001f) return v1.Normalized();

		// fail no distance passed
		if (mPoints.size() >= 2) return (mPoints[0] - mPoints[mPoints.size()-1]).Normalized();

		// no possible solution
		return T();
	}

	// Get the distances
	float GetDistanceAtTime(float time) const { return time * mTotalLength; }
	float GetLength() const { return mTotalLength; }

	// Set approximations
	void SetApproximations(short approximations){mApproximations = approximations; EvaluateLength();}
	short GetApproximations()const{return mApproximations;}

	// Get SPline Type
	void SetType(SplineType type){mType = type; EvaluateLength();} 
	SplineType GetType() const { return mType; }

	// Get the points
	std::vector<T>& GetPoints()const{return mPoints;}
	int GetPointCount()const{return (int)mPoints.size();}
	T GetPoint(int idx)const{return mPoints[idx];}
	
private:
	// member functions
	void GetSegmentIdxAndT(float distance, int& idx, float& t) const 
	{
		idx = 0;
		int points = (int)mSegmentLength.size();
		while (distance > mSegmentLength[idx] && idx < points-1)
		{
			distance -= mSegmentLength[idx++];
		}

		t = Clamp(0.0f, 1.0f, distance / mSegmentLength[idx]);
	}

	T GetSegmentSample(float t, T* points) const 
	{
		switch(mType)
		{
			case SPLINE_LINEAR:
			{
				return Lerp(points[0], points[1], t);
			}

			case SPLINE_BEZIER_SEGMENTS_4:
			{
				float rT = 1.0f - t;
				T p0 = points[0] * (rT*rT*rT);
				T p1 = points[1] * (rT*rT*t*3.0f);
				T p2 = points[2] * (rT*t*t*3.0f);
				T p3 = points[3] * (t*t*t);
				return p0 + p1 + p2 + p3;
			}

			case SPLINE_BEZIER_QUAD:
			{
				if (t < 0.5f)
				{
					if ((points[0] - points[1]).SqrLength() > 0.001f)
					{
						T v2 = Lerp(points[0], points[1], t*0.5f+0.75f);
						T v1 = Lerp(points[1], points[2], t*0.5f+0.25f);
						return Lerp(v2, v1, t+0.5f);
					}
					else
					{
						return Lerp(points[1], points[2], t);
					}
				}
				else
				{
					if ((points[2] - points[3]).SqrLength() > 0.001f)
					{
						T v1 = Lerp(points[1], points[2], t*0.5f+0.25f);
						T v2 = Lerp(points[2], points[3], t*0.5f-0.25f);
						return Lerp(v2, v1, 1.5f - t);
					}
					else
					{
						return Lerp(points[1], points[2], t);
					}
				}
			}

			case SPLINE_CATMULLROM:
			{
				float t2 = t*t;
				float t3 = t*t*t;
				float tension = 0.0f;

				T& start = points[1];
				T& end = points[2];
				T startTangent = (1.0f - tension) * (points[2] - points[0]) * 0.5f;
				T endTangent = (1.0f - tension) * (points[3] - points[1]) * 0.5f;

				start *= ((2.0f * t3) - (3.0f * t2)+1.0f);
				end *= ((-2.0f*t3) + (3.0f*t2)) ;
				startTangent *= ((t3) - (2.0f*t2)+t);
				endTangent *= ((t3) - (t2));

				return start + end + startTangent + endTangent;
			}
		}

		// fail
		return T();
	}

	T GetSample(float distance) const 
	{
		int idx; float t;
		GetSegmentIdxAndT(distance, idx, t);
		switch(mType)
		{
			case SPLINE_LINEAR:
			{
				T points[2];
				points[0] = mPoints[idx];
				points[1] = mPoints[idx+1];
				return GetSegmentSample(t, &points[0]);
			}

			case SPLINE_BEZIER_SEGMENTS_4:
			{
				int pi = idx * 4;
				T points[4];
				points[0] = mPoints[pi];
				points[1] = mPoints[pi+1];
				points[2] = mPoints[pi+2];
				points[3] = mPoints[pi+3];
				return GetSegmentSample(t, &points[0]);
			}

			case SPLINE_BEZIER_QUAD:
			case SPLINE_CATMULLROM:
			{
				T points[4];
				points[0] = mPoints[Max(0, idx - 1)];
				points[1] = mPoints[idx];
				points[2] = mPoints[idx + 1];
				points[3] = mPoints[Min(int(mPoints.size())-1, idx + 2)];
				return GetSegmentSample(t, &points[0]);
			}
		}

		// fail
		return T();
	}

	float GetSegmentLength(int idx) const 
	{
		switch(mType)
		{
			case SPLINE_LINEAR:
			{
				return (mPoints[idx] - mPoints[idx+1]).Length();
			}

			case SPLINE_BEZIER_SEGMENTS_4:
			{
				float step = 1.0f / float(mApproximations);
				int pi = idx * 4;
				float t = 0.0f;
				float len = 0.0f;
				T points[4];
				points[0] = mPoints[pi];
				points[1] = mPoints[pi+1];
				points[2] = mPoints[pi+2];
				points[3] = mPoints[pi+3];
				T lastPos = GetSegmentSample(t, &points[0]);
				for (int i=1; i<mApproximations; ++i)
				{
					t += step;
					T pos = GetSegmentSample(t, &points[0]);
					len += (lastPos - pos).Length();
					lastPos = pos;
				}
				return len;
			}

			case SPLINE_BEZIER_QUAD:
			case SPLINE_CATMULLROM:
			{
				float step = 1.0f / float(mApproximations);
				float t = 0.0f;
				float len = 0.0f;
				T points[4];
				points[0] = mPoints[Max(0, idx - 1)];
				points[1] = mPoints[idx];
				points[2] = mPoints[idx + 1];
				points[3] = mPoints[Min(int(mPoints.size())-1, idx + 2)];
				T lastPos = GetSegmentSample(t, &points[0]);
				for (int i=1; i<mApproximations; ++i)
				{
					t += step;
					T pos = GetSegmentSample(t, &points[0]);
					len += (lastPos - pos).Length();
					lastPos = pos;
				}
				return len;
			}
		}

		// fail
		return 1.0f;
	}
	// member variables
	std::vector<T> mPoints;
	std::vector<float> mSegmentLength;
	float mTotalLength;
	short mApproximations;
	SplineType mType;
};

typedef Spline<Vec2f> Spline2f;
typedef Spline<Vec3f> Spline3f;
typedef Spline<Orientationf> SplineOf;
}